#pragma once

#include <mutex>
#include <string>
#include <memory>
#include <sstream>
#include <glog/logging.h>
#include <condition_variable>
#include <jsoncpp/json/json.h>

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include "yhs_msgs/bms_fb.h"
#include "basic/tools.h"
#include "ros/udp_client.h"

class ROS1Node {
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
public:
    struct OccupyMap
    {
        int width;
        int height;
        float resolution;
        float origin_x;
        float origin_y;
        float origin_z;
        float quat_x;
        float quat_y;
        float quat_z;
        float quat_w;
        std::string filename;
    };

public:
    ROS1Node(std::shared_ptr<ros::NodeHandle> nh, std::string ipv4, uint16_t port);

    void thread_UDPRecv();
    void thread_op();
    void thread_UDPSend();
    void stop();
    void set_map_path(const std::string str) { _map_path = str;}
    std::string get_map_path() {return _map_path;}
private:
    void launch_cmd(int cmd);
    void publish_vel(float vel, float yaw);
    void publish_goal(float x, float y, float yaw);
    void publish_init_pose(float x, float y, float yaw);
    void battery_cb(const yhs_msgs::bms_fb msg);
    void odometry_cb(const nav_msgs::Odometry::ConstPtr &msg);
    void occup_cb(const nav_msgs::OccupancyGrid::ConstPtr &msg);

    std::shared_ptr<ros::NodeHandle> _nh;
    // 通信
    std::mutex _mtx_udp;
    std::mutex _mtx_map;
    std::mutex _mtx_occupy;
    std::mutex _mtx_message;
    std::condition_variable _cond;
    std::string _message = "";
    std::shared_ptr<UDPClient> _updClient;
    // JSON
    Json::Value _va;
    Json::Value root;
    std::stringstream _sstream;
    Json::FastWriter _styled_writer;
    std::shared_ptr<Json::Reader> _re;
    // 发布
    ros::Publisher pub_vel;
    ros::Publisher pub_goal;
    ros::Publisher pub_initPose;
    std::shared_ptr<MoveBaseClient> ac;

    // 订阅
    float _pose[7] = {0};
    float _remaining_capacity = 0;
    bool bBattery = false;
    bool bOdometry = false;
    bool bGoal = false;
    std::string GoalState = "";
    const int nOCcup = 2;
    int bOccup = 0;
    bool bSaveMap = false;
    struct OccupyMap _occupy;
    struct OccupyMap _map;
    ros::Subscriber sub_battery;
    ros::Subscriber sub_odom;
    ros::Subscriber sub_occup;

    // nav
    bool isNavState = true;
    tf::StampedTransform transform;
    tf::TransformListener listener;
    std::string _map_path;
};

